Descripción del problema
Se implementará un filtro de Kalman para fusionar los datos de sensores presentes en un vehículo para estimar la posición y orientación del mismo en dos dimensiones. Un diagrama del mismo se presenta en la Figura 1 .
En la figura se representan los ejes \(x\) e \(y\) , llamados el sistema de referencia inercial. Se repesenta además el eje \(x_b\) , el cual junto con un eje \(y_b\) ortogonal al mismo forman el sistema de referencia del móvil, llamado body . El filtro de Kalman estimará la posición del móvil en el sistema de referencia intercial, \(x_{est},\, y_{est}\) así como la orientación del mismo, dada por el ángulo \(\theta\) .
Los instrumentos disponibles para la estimación de la posición son los siguientes.
Unidad de Medición Inercial (IMU): Consiste en un giróscopo y un acelerómetro los cuales miden a una tasa \(f_s\) .
Acelerómetro: Mide la aceleración lineal del móvil en el sistema body , \(u^{x_b} = \ddot{x}_b\) y \(u^{y_b} = \ddot{y}_b\) .
Giróscopo: Mide la velocidad angular del móvil, \(u^{\theta} = \dot{\theta}\) .
GPS: Mide la posición del móvil en el sistema de referencia inercial a una tasa \(f_s'\) .
Magnetómetro: Por medio de mediciones del campo magnético terrestre calcula la orientación del móvil, también a tasa \(f_s'\) .
Error de los instrumentos
Para cada dimensión de medición se considera su correspondiente error
Código
using LinearAlgebra
using Distributions
using Plots
using PlotThemes
theme (: wong2)
## Parametros de error instrumentos
Δt = 0.1
fs = 10
fs2 = 1
PSD_a = 80 * 10 * 1e-6
PSD_g = 0.03 * π / 180
PSD_m = 100e-6 * 1e-4
B_T = 25358e-9
err_IMU_x = PSD_a*sqrt (fs)
err_IMU_θ = PSD_g*sqrt (fs)
err_GPS = 2.5
err_MAG = PSD_m*sqrt (fs2)/ B_T
Q = [err_IMU_x^ 2 0 0 ; 0 err_IMU_x^ 2 0 ; 0 0 err_IMU_θ^ 2 ]
R = [err_GPS^ 2 0 0 ; 0 err_GPS^ 2 0 ; 0 0 err_MAG^ 2 ]
dist_ruido_IMU = MvNormal ([0 ,0 ,0 ], Q)
dist_ruido_GPS = MvNormal ([0 ,0 ,0 ], R)
Filtro de Kalman
El filtro de Kalman se hará sobre el estimador del estado de posición, velocidad, y orientación del móvil en el sistema de referencia inercial
\[
\mathbf{\hat{x}}_k = \begin{bmatrix}\hat{x}_k \\ \hat{\dot{x}}_k \\ \hat{y}_k \\ \hat{\dot{y}}_k \\ \hat{\theta}_k \end{bmatrix}
\]
Medición del IMU
A cada instante de muestreo del IMU se obtiene un vector de mediciones \(\mathbf{\overline{u}}_k\) , el cual se define como su valor real \(\mathbf{u}_k\) más su error \(\mathbf{w'}_k\)
\[
\mathbf{\overline{u}}_k =
\mathbf{u}_k + \mathbf{w}_k' = \begin{bmatrix}u_k^{x_b} \\ u_k^{y_b} \\
u_k^{\theta} \end{bmatrix} + \mathbf{w}_k'
\]
Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{w}'\) es dada por
\[
Q = \begin{bmatrix}
(\sigma^{x_b})^2 & 0 & 0\\
0 & (\sigma^{x_b})^2 & 0\\
0 & 0 & (\sigma^\theta)^2\\
\end{bmatrix}
\]
Medición del GPS/Magnetómetro
Cuando \(k\) corresponde a un instante de muestreo del GPS, se obtiene un vector de mediciones \(\mathbf{\overline{z}}_k\) , el cual se define como su valor real \(\mathbf{z}_k\) más su error \(\mathbf{v}_k\)
\[
\mathbf{\overline{z}}_k =
\mathbf{z}_k + \mathbf{v}_k = \begin{bmatrix}z_k^{x} \\ z_k^{y} \\
z_k^{\theta} \end{bmatrix} + \mathbf{v}_k
\]
En ausencia de aceleración, la evolución del estado en un tiempo \(\Delta t\) seguiría un movimiento rectilíneo uniforme, dado por
\[
\mathbf{x}_k = \begin{bmatrix}x_k \\ \dot{x}_k \\ y_k \\ \dot{y}_k \\ \theta_k \end{bmatrix} =
\begin{bmatrix}
1 & \Delta t & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 \\
0 & 0 & 1 & \Delta t & 0 \\
0 & 0 & 0 & 1 & 0 \\
0 & 0 & 0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}x_{k-1} \\ \dot{x}_{k-1} \\ y_{k-1} \\ \dot{y}_{k-1} \\ \theta_{k-1} \end{bmatrix}
\]
Modelo de actualización de estado
La actualización del estado se hace en función de las mediciones de la IMU, que informan la aceleración del móvil en el sistema de referencia body , estas se modelan como el vector de medición \(\mathbf u\) más el vector de error \(\mathbf w'\)
Estas aceleraciones se transladan al sistema de referencia inercial y se utilizan para realizar la actualización por modelo del filtro de Kalman, dada por medio de la ecuación
\[
\mathbf{\hat{x}}_k = A\mathbf{\hat{x}}_{k-1} + B_{k-1} \mathbf{u}_{k-1} + \mathbf{w}_{k-1}
\]
En donde la matriz \(B_{k-1}\) que obtiene la actualización del estado en el sistema inercial \(\mathbf{x}\) en función de las mediciones de aceleración en el sistema body es dada por
\[
B_{k-1} = \begin{bmatrix} \frac 1 2 \Delta t^2 \cos(\theta_{k-1}) &
- \frac 1 2 \Delta t^2 \sin(\theta_{k-1}) &
0 \\
\Delta t \cos(\theta_{k-1}) &
- \Delta t \sin(\theta_{k-1}) &
0 \\
\frac 1 2 \Delta t^2 \sin(\theta_{k-1}) &
\frac 1 2 \Delta t^2 \cos(\theta_{k-1}) &
0 \\
\Delta t \sin(\theta_{k-1}) &
\Delta t \cos(\theta_{k-1}) &
0 \\
0 & 0 & \Delta t
\end{bmatrix}
\]
Y el ruido es similarmente transformado, obteniendo \(\mathbf{w}_{k-1} = B \mathbf{w}_{k-1}'\) . La matriz de covarianza que resulta de la transformación es dada por
\[
Q_k = B Q B^T
\]
Modelo de actualización por medición
Código
A = [1 Δt 0 0 0 ;
0 1 0 0 0 ;
0 0 1 Δt 0 ;
0 0 0 1 0 ;
0 0 0 0 1 ]
function B_matrix (θ)
return [Δt^ 2 *cos (θ)/ 2 - Δt^ 2 *sin (θ)/ 2 0 ;
Δt*cos (θ) -Δt*sin (θ) 0 ;
Δt^ 2 *sin (θ)/ 2 Δt^ 2 *cos (θ)/ 2 0 ;
Δt*sin (θ) Δt*cos (θ) 0 ;
0 0 Δt]
end
H = [1 0 0 0 0 ;
0 0 1 0 0 ;
0 0 0 0 1 ]
Implementación del algoritmo de estimación
Inicialización
Código
function kalman_initialize (t, z)
N = length (t)
x = zeros (5 , N)
x[1 : 2 : 5 ,1 ] = z[: ,1 ]
P = zeros (5 , 5 , N)
P[: ,: ,1 ] = diagm ([err_GPS^ 2 ,(err_GPS* fs2)^ 2 ,err_GPS^ 2 ,(err_GPS* fs2)^ 2 ,2 pi])
K = zeros (5 , 3 , N)
return x, P, K
end
kalman_initialize (generic function with 1 method)
Predicción
En cada instante de muestreo del IMU se realiza la etapa de predicción
Estimación a priori : \(\mathbf{\hat{x}}_k^{-} = \mathbf{\hat{x}}_{k-1} + B_{k-1} \mathbf{\overline{u}}_{k-1}\)
Covarianza a priori : \(P_k^{-} = A P_{k-1}A^T + Q_k\)
Ganancia de Kalman: \(K_k = P^{-}_k H^T (H P_k^{-}H^T + R_k)^{-1}\)
Código
function kalman_predict (x, u, P)
B = B_matrix (x[5 ])
x_est = A* x + B* u
P_est = A* P* A' + B* Q* B'
K = P_est* H' *inv (H* P_est* H'+ R)
return x_est, P_est, K
end
kalman_predict (generic function with 1 method)
Corrección
En cada instante de muestreo del GPS/magnetómetro, se aplica una corrección por medición al estimador.
Estimador a posteriori : \(\mathbf{\hat{x}}_k^{+} = \mathbf{\hat{x}}_k^{-} + K_k (\mathbf{\overline{z}}_k-H\mathbf{\hat{x}}_k^{-})\)
Covarianza a posteriori : \(P_k^{+} = (I-K_k H)P_k^{-}\)
Código
function kalman_correct (x_prior, z, P_prior, K)
x_est = x_prior + K* (z- H* x_prior)
P_est = (I- K* H)* P_prior
return x_est, P_est
end
kalman_correct (generic function with 1 method)
Código
function kalman (t, tasa_GPS, u, z)
x, P, K = kalman_initialize (t, z)
for k in 2 : length (t)
x_est, P_est, K_k = kalman_predict (x[: ,k- 1 ], u[: ,k], P[: ,: ,k- 1 ])
if k% tasa_GPS == 0
x_est, P_est = kalman_correct (x_est, z[: ,k], P_est, K_k)
end
x[: ,k] = x_est
P[: ,: ,k] = P_est
K[: ,: ,k] = K_k
end
return x, P, K
end
kalman (generic function with 1 method)
Estimación de diferentes trayectorias
En todas las trayectorias, los ruidos de los instrumentos se modelan como variables aleatorias gaussianas de media \(0\) , con muestras independientes e idénticamente distribuidas.
\[
\mathbf{w}'_k \sim \mathcal{N}\left[\mathbf{0}, Q\right] \qquad \mathbf{v}_k \sim \mathcal{N}\left[\mathbf{0}, R\right]
\]
Código
T = 500
t = 0 : 0.1 : T
u_noise = hcat ([rand (dist_ruido_IMU) for ti in t]... )
z_noise = hcat ([rand (dist_ruido_GPS) for ti in t]... )
Móvil estático
\[
\mathbf{\overline{u}}_k = \begin{bmatrix} 0 \\ 0 \\
0 \end{bmatrix} + \mathbf{w}_k'
\]
\[
\mathbf{\overline{z}}_k = \begin{bmatrix} 0 \\ 0 \\
0 \end{bmatrix} + \mathbf{v}_k
\]
Código
function estatico_IMU ()
return [0 ,0 ,0 ]
end
function estatico_GPS ()
return [0 ,0 ,0 ]
end
u_true = hcat ([estatico_IMU () for ti in t]... )
z_true = hcat ([estatico_GPS () for ti in t]... )
u = u_true + u_noise
z = z_true + z_noise
x, P, K = kalman (t, 10 , u, z)
Código
function trayectoria_xy (x, z_true)
p1 = plot (x[1 ,: ], x[3 ,: ], label= "Estimación" , xlabel= "x" , ylabel= "y" , aspect_ratio=: equal, title= "Trayectoria XY" )
plot! (p1, z_true[1 ,: ], z_true[2 ,: ], label= "Trayectoria" )
return p1
end
trayectoria_xy (x, z_true)
Código
function trayectoria_t (t, x, z_true)
p1 = plot (t, x[1 ,: ], ylabel= "x [m]" , title= "Trayectoria en función del tiempo" )
plot! (p1, t, z_true[1 ,: ])
p2 = plot (t, x[3 ,: ], ylabel= "y [m]" )
plot! (p2, t, z_true[2 ,: ])
p3 = plot (t, x[5 ,: ] .% 2 pi, ylabel= "θ" , xlabel= "t [s]" )
plot! (p3, t, z_true[3 ,: ] .% 2 pi)
return plot (p1,p2,p3, layout= (3 ,1 ), legend= false )
end
trayectoria_t (t, x, z_true)
Análisis de desempeño
Error de estimación
Se procede a cuantificar el error de la estimación, el cual se define como la norma cuadrática de la distancia entre la estimación y el valor real
\[
\epsilon_k = \left\lVert\begin{bmatrix}\hat{x}_k\\\hat{y}_k\end{bmatrix}-
\begin{bmatrix}z^x_k\\ z^y_k\end{bmatrix}\right\rVert^2
\]
Código
function error_cuadratico (x, z_true)
xs = [[x_i[1 ] x_i[3 ]] for x_i in eachcol (x)]
zs = [[z_i[1 ] z_i[2 ]] for z_i in eachcol (z_true)]
return [norm (x_i- z_i)^ 2 for (x_i, z_i) in zip (xs, zs)]
end
error_cuadratico (generic function with 1 method)
Esto se compara con la incerteza del estimador, la cual se cuantifica con la traza de la matriz de covarianza \(P\)
Código
function varianza_estimador (P)
return [tr (P[: ,: ,i]) for i in 1 : size (P,3 )]
end
varianza_estimador (generic function with 1 method)
Código
function plot_error (t, x, z_true, P)
v = varianza_estimador (P)
e = error_cuadratico (x, z_true)
p1 = plot (t, v, ylim= [0 , maximum (v)* 1.1 ], ylabel= "Incerteza" )
p2 = plot (t, e , ylim= [0 , maximum (e )* 1.1 ], ylabel= "Error cuadrático" , xlabel= "t" )
return plot (p1, p2, layout= (2 ,1 ))
end
plot_error (t, x, z_true, P)
Ganancia de Kalman
Código
function kalman_gain (K)
return [norm (K[: ,: ,i]) for i in 1 : size (K,3 )]
end
kalman_gain (generic function with 1 method)
Código
function plot_gain (t, P, K)
v = varianza_estimador (P)
k = kalman_gain (K)
p1 = plot (t, v, ylim= [0 , maximum (v)* 1.1 ], ylabel= "Incerteza" )
p2 = plot (t, k, ylim= [0 , maximum (k)* 1.1 ], ylabel= "Ganancia Kalman" , xlabel= "t" )
return plot (p1, p2, layout= (2 ,1 ))
end
plot_gain (t, P, K)
Código
k1 = 4000
k2 = 4050
plot_gain (t[k1: k2], P[: ,: ,k1: k2], K[: ,: ,k1: k2])
Efecto de la incerteza de los instrumentos
Incrementa x5 error de GPS
Código
Q = [err_IMU_x^ 2 0 0 ; 0 err_IMU_x^ 2 0 ; 0 0 err_IMU_θ^ 2 ]
R = [(5 * err_GPS)^ 2 0 0 ; 0 (5 * err_GPS)^ 2 0 ; 0 0 err_MAG^ 2 ]
dist_ruido_IMU = MvNormal ([0 ,0 ,0 ], Q)
dist_ruido_GPS = MvNormal ([0 ,0 ,0 ], R)
u_noise = hcat ([rand (dist_ruido_IMU) for ti in t]... )
z_noise = hcat ([rand (dist_ruido_GPS) for ti in t]... )
u_true = hcat ([mrua_IMU (5e-4 ) for ti in t]... )
z_true = hcat ([mrua_GPS (5e-4 , ti, pi / 4 ) for ti in t]... )
u = u_true + u_noise
z = z_true + z_noise
x, P, K = kalman (t, 10 , u, z)
([6.399781199194112 6.3997990738809545 … 45.341589260594674 45.36055364889119; 0.0 0.00035749373686201424 … 0.18985890889783308 0.18942885703251805; … ; 0.0 0.00021385783699033828 … 0.1557259904736974 0.15562015104786917; 0.7848488405492863 0.7849244225793554 … 0.7857678701547156 0.7854209220538022], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.31250000016 0.6250000032 … -3.269996546517461e-26 0.0; 0.6250000032 6.250000064 … 4.027810561325809e-24 0.0; … ; 1.5455836360409911e-25 -4.5202712722881835e-24 … 6.250000064 0.0; 0.0 0.0 … 0.0 6.283185334595154;;; 6.5000000016 1.2500000128000002 … 4.860834460651788e-25 0.0; 1.2500000128000002 6.250000128 … 3.802909870361181e-24 0.0; … ; -4.120565610610859e-25 -4.7063484492424615e-24 … 6.250000128 0.0; 0.0 0.0 … 0.0 6.283185362010721;;; … ;;; 1.789434126214376 0.010048778940915963 … 1.107835787193436e-21 0.0; 0.010048778940915964 0.00011380649969204478 … 2.3689206546853487e-23 0.0; … ; 1.9286714934979128e-21 7.105349407547785e-24 … 0.00011380649969204478 0.0; 0.0 0.0 … 0.0 3.575038139502067e-7;;; 1.7711384781044608 0.009946127965829606 … 1.1028462603811658e-21 0.0; 0.009946127965829608 0.00011323011782660337 … 2.266065265163026e-23 0.0; … ; 1.8874621364388075e-21 5.740016248312654e-24 … 0.00011323011782660337 0.0; 0.0 0.0 … 0.0 1.1076370392297255e-7;;; 1.773128836158805 0.009957454177612266 … 1.1049421360554004e-21 0.0; 0.009957454177612268 0.00011329411782660338 … 2.580537468853133e-23 0.0; … ; 1.8877920421337128e-21 7.9209010990109e-24 … 0.00011329411782660338 0.0; 0.0 0.0 … 0.0 1.3817927170377632e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.038831218762960626 -5.659833239126597e-29 0.0; 0.0038446751446328943 -6.915408885968804e-28 0.0; … ; 9.505566154937311e-28 0.0038446751446328943 0.0; 0.0 0.0 0.9999999752491497;;; 0.03993855607702672 9.813851658963173e-29 0.0; 0.007680491630032021 2.580844671204317e-27 0.0; … ; -2.530169321216744e-27 0.007680491630032021 0.0; 0.0 0.0 0.9999999752491497;;; … ;;; 0.011322706488465957 -5.024560499576099e-22 0.0; 6.358399722496316e-5 7.001907088186049e-24 0.0; … ; 1.2075519151180111e-23 6.358399722496316e-5 0.0; 0.0 0.0 0.6968640780830011;;; 0.011335286259868549 -5.010596696256948e-22 0.0; 6.365521898130948e-5 7.016279173975292e-24 0.0; … ; 1.2079757673208367e-23 6.365521898130948e-5 0.0; 0.0 0.0 0.7122418117142865;;; 0.0112206918646524 -4.885008084320078e-22 0.0; 6.301263777618486e-5 6.984084008668774e-24 0.0; … ; 1.1821788768628917e-23 6.301263777618486e-5 0.0; 0.0 0.0 0.47048808605167836])
Código
trayectoria_xy (x, z_true)
Código
trayectoria_t (t, x, z_true)
Código
u_true = hcat ([circular_IMU (20 , 2 * pi / T, ti) for ti in t]... )
z_true = hcat ([circular_GPS (20 , 2 * pi / T, ti) for ti in t]... )
u = u_true + u_noise
z = z_true + z_noise
x, P, K = kalman (t, 10 , u, z)
([26.399781199194113 26.399770484752086 … 21.137718470315665 21.139276033568887; 0.0 -0.00021428884050346046 … 0.015848432720687408 0.015302832343740299; … ; 0.0 0.00035418007102216406 … 0.23224185555942428 0.23181349171664725; 1.5702470039467347 1.5715792230382397 … 7.8530947036703145 7.854004392630837], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.31250000016 0.6250000032 … 1.6087096783876353e-28 0.0; 0.6250000032 6.250000064 … 7.256472083863048e-27 0.0; … ; -2.5780275752747166e-28 -3.492159999452689e-27 … 6.250000064 0.0; 0.0 0.0 … 0.0 6.283185334595154;;; 6.5000000016 1.2500000128000002 … 8.119845002821807e-28 0.0; 1.2500000128000002 6.250000128 … 3.938911542442671e-27 0.0; … ; -4.297693240627179e-28 2.5472535823627876e-27 … 6.250000128 0.0; 0.0 0.0 … 0.0 6.283185362010721;;; … ;;; 1.789434126214376 0.010048778940915963 … -1.084727966180855e-21 0.0; 0.010048778940915964 0.00011380649969204478 … -2.2408183550581983e-23 0.0; … ; 4.222681407684344e-21 9.498701955016883e-23 … 0.00011380649969204478 0.0; 0.0 0.0 … 0.0 3.575038139502067e-7;;; 1.7711384781044608 0.009946127965829606 … -1.067773155787924e-21 0.0; 0.009946127965829608 0.00011323011782660337 … -2.2285538000289733e-23 0.0; … ; 4.166316871199004e-21 9.446469265752443e-23 … 0.00011323011782660337 0.0; 0.0 0.0 … 0.0 1.1076370392297255e-7;;; 1.773128836158805 0.009957454177612266 … -1.0700015261885246e-21 0.0; 0.009957454177612268 0.00011329411782660338 … -2.228936724195128e-23 0.0; … ; 4.175763602472809e-21 9.446973240542751e-23 … 0.00011329411782660338 0.0; 0.0 0.0 … 0.0 1.3817927170377632e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.038831218762960626 1.0065668248689088e-31 0.0; 0.0038446751446328943 1.2244906599188314e-30 0.0; … ; -1.5856425077993764e-30 0.0038446751446328943 0.0; 0.0 0.0 0.9999999752491497;;; 0.03993855607702672 7.872090306349743e-31 0.0; 0.007680491630032021 5.335970871632301e-30 0.0; … ; -2.6365881045100373e-30 0.007680491630032021 0.0; 0.0 0.0 0.9999999752491497;;; … ;;; 0.011322706488465957 -6.819089261947242e-22 0.0; 6.358399722496316e-5 -6.907035452039849e-24 0.0; … ; 2.66051354608374e-23 6.358399722496316e-5 0.0; 0.0 0.0 0.6968640780830011;;; 0.011335286259868549 -6.832587527973631e-22 0.0; 6.365521898130948e-5 -6.920990849239914e-24 0.0; … ; 2.666442797567362e-23 6.365521898130948e-5 0.0; 0.0 0.0 0.7122418117142865;;; 0.0112206918646524 -6.693576799048734e-22 0.0; 6.301263777618486e-5 -6.814780531329688e-24 0.0; … ; 2.6313887273236363e-23 6.301263777618486e-5 0.0; 0.0 0.0 0.47048808605167836])
Código
trayectoria_xy (x, z_true)
Código
trayectoria_t (t, x, z_true)
Código
plot_error (t, x, z_true, P)
Código
plot_gain (t[k1: k2], P[: ,: ,k1: k2], K[: ,: ,k1: k2])
Incrementa x5 error de IMU
Código
Q = [(5 * err_IMU_x)^ 2 0 0 ; 0 (5 * err_IMU_x)^ 2 0 ; 0 0 (5 * err_IMU_θ)^ 2 ]
R = [err_GPS^ 2 0 0 ; 0 err_GPS^ 2 0 ; 0 0 err_MAG^ 2 ]
dist_ruido_IMU = MvNormal ([0 ,0 ,0 ], Q)
dist_ruido_GPS = MvNormal ([0 ,0 ,0 ], R)
u_noise = hcat ([rand (dist_ruido_IMU) for ti in t]... )
z_noise = hcat ([rand (dist_ruido_GPS) for ti in t]... )
u_true = hcat ([mrua_IMU (5e-4 ) for ti in t]... )
z_true = hcat ([mrua_GPS (5e-4 , ti, pi / 4 ) for ti in t]... )
u = u_true + u_noise
z = z_true + z_noise
x, P, K = kalman (t, 10 , u, z)
([-4.053193522251121 -4.053107450389068 … 44.57735438453592 44.596830676069956; 0.0 0.0017214372410541373 … 0.19484425526926719 0.19468157541137582; … ; 0.0 -0.0008373128802501247 … 0.23659620376470644 0.23761825355134944; 0.7855433303676169 0.786645576427757 … 0.7857587554823343 0.7868058466499893], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.312500004 0.62500008 … 9.529706604467709e-24 0.0; 0.62500008 6.2500016 … 1.572470425413841e-23 0.0; … ; 5.5265649441002705e-24 2.8199545226197805e-24 … 6.2500016 0.0; 0.0 0.0 … 0.0 6.2831859925687805;;; 6.500000040000001 1.25000032 … 1.7739246973428253e-23 0.0; 1.25000032 6.2500032 … 7.593671972830343e-23 0.0; … ; 2.3009522671901827e-26 -1.2076188861814014e-22 … 6.2500032 0.0; 0.0 0.0 … 0.0 6.283186677957975;;; … ;;; 0.36169288198748656 0.010229584529472202 … -1.8396903206497448e-21 0.0; 0.010229584529472202 0.0005721606635760474 … 6.823817069246597e-23 0.0; … ; 2.3993859677098043e-21 2.6112634200199806e-22 … 0.0005721606635760474 0.0; 0.0 0.0 … 0.0 6.320639916249882e-6;;; 0.3437392039718934 0.009721119932251493 … -1.6289303424767417e-21 0.0; 0.009721119932251493 0.0005577606635759174 … 7.340192039930846e-23 0.0; … ; 2.190209028955388e-21 2.7648033253749534e-22 … 0.0005577606635759174 0.0; 0.0 0.0 … 0.0 1.5213716556903334e-7;;; 0.34568900956497944 0.009776975998609085 … -1.6190272842674518e-21 0.0; 0.009776975998609085 0.0005593606635759175 … 6.176572960357224e-24 0.0; … ; 2.2184080698883454e-21 3.855958732282835e-22 … 0.0005593606635759175 0.0; 0.0 0.0 … 0.0 8.375263600891277e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.5024875623474667 4.044407862005418e-27 0.0; 0.049751250133412535 -2.7043607576388794e-25 0.0; … ; 4.405483851296487e-25 0.049751250133412535 0.0; 0.0 0.0 0.9999999752491522;;; 0.5098039231064976 3.5591829840556046e-26 0.0; 0.09803924047673965 2.970478437021516e-25 0.0; … ; 4.032043718105931e-27 0.09803924047673965 0.0; 0.0 0.0 0.999999975249155;;; … ;;; 0.054705033709726854 -9.921045094305651e-21 0.0; 0.001547195962072148 -2.707088229923479e-22 0.0; … ; 3.4665418277225106e-22 0.001547195962072148 0.0; 0.0 0.0 0.9759866455818303;;; 0.05499827263550295 -9.968180513867565e-21 0.0; 0.0015553791891602389 -2.6961327433362486e-22 0.0; … ; 3.5043344463286204e-22 0.0015553791891602389 0.0; 0.0 0.0 0.9782848225201974;;; 0.05241135673068665 -8.998144244783633e-21 0.0; 0.0014823282274877797 -2.4044713562199325e-22 0.0; … ; 3.2221526381863383e-22 0.0014823282274877797 0.0; 0.0 0.0 0.8433959372095443])
Código
trayectoria_xy (x, z_true)
Código
trayectoria_t (t, x, z_true)
Código
u_true = hcat ([circular_IMU (20 , 2 * pi / T, ti) for ti in t]... )
z_true = hcat ([circular_GPS (20 , 2 * pi / T, ti) for ti in t]... )
u = u_true + u_noise
z = z_true + z_noise
x, P, K = kalman (t, 10 , u, z)
([15.94680647774888 15.946881152223028 … 20.167144093856205 20.166095811845853; 0.0 0.001493489482977554 … -0.009906401051208671 -0.011059239155777875; … ; 0.0 0.0005751244837655708 … 0.3073835570019056 0.3079425585645642; 1.5709414937650652 1.5733003768866411 … 7.853085588997933 7.855389317227024], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.312500004 0.62500008 … -1.1829558670837016e-27 0.0; 0.62500008 6.2500016 … 1.5743550327490137e-26 0.0; … ; -1.5598932487744637e-27 -2.1465242046224327e-26 … 6.2500016 0.0; 0.0 0.0 … 0.0 6.2831859925687805;;; 6.500000040000001 1.25000032 … -1.224802040086607e-26 0.0; 1.25000032 6.2500032 … -3.2902646932283077e-26 0.0; … ; -1.31818563583822e-26 -3.978384850805885e-26 … 6.2500032 0.0; 0.0 0.0 … 0.0 6.283186677957975;;; … ;;; 0.36169288198748656 0.010229584529472202 … -6.6133369889639375e-21 0.0; 0.010229584529472202 0.0005721606635760474 … -4.308123134440125e-22 0.0; … ; 5.50852070862659e-21 3.353908489802122e-22 … 0.0005721606635760474 0.0; 0.0 0.0 … 0.0 6.320639916249882e-6;;; 0.3437392039718934 0.009721119932251493 … -6.115763448215439e-21 0.0; 0.009721119932251493 0.0005577606635759174 … -4.103956056185978e-22 0.0; … ; 5.070884373413702e-21 3.187089130763981e-22 … 0.0005577606635759174 0.0; 0.0 0.0 … 0.0 1.5213716556903334e-7;;; 0.34568900956497944 0.009776975998609085 … -6.156801209714805e-21 0.0; 0.009776975998609085 0.0005593606635759175 … -4.104388227819965e-22 0.0; … ; 5.1027683570758455e-21 3.187561317863888e-22 … 0.0005593606635759175 0.0; 0.0 0.0 … 0.0 8.375263600891277e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.5024875623474667 2.1229816298321444e-31 0.0; 0.049751250133412535 1.2032831145192752e-28 0.0; … ; -1.2414004669719228e-28 0.049751250133412535 0.0; 0.0 0.0 0.9999999752491522;;; 0.5098039231064976 -8.354453207068883e-30 0.0; 0.09803924047673965 -3.131300859215219e-28 0.0; … ; -1.0351894275620513e-27 0.09803924047673965 0.0; 0.0 0.0 0.999999975249155;;; … ;;; 0.054705033709726854 -1.678954910837624e-20 0.0; 0.001547195962072148 -9.830832489442495e-22 0.0; … ; 8.069179262857482e-22 0.001547195962072148 0.0; 0.0 0.0 0.9759866455818303;;; 0.05499827263550295 -1.696971285252791e-20 0.0; 0.0015553791891602389 -9.88833768490497e-22 0.0; … ; 8.113414997461925e-22 0.0015553791891602389 0.0; 0.0 0.0 0.9782848225201974;;; 0.05241135673068665 -1.541475971729123e-20 0.0; 0.0014823282274877797 -9.19117477450877e-22 0.0; … ; 7.507072213514851e-22 0.0014823282274877797 0.0; 0.0 0.0 0.8433959372095443])
Código
trayectoria_xy (x, z_true)
Código
trayectoria_t (t, x, z_true)
Código
plot_error (t, x, z_true, P)
Código
plot_gain (t[k1: k2], P[: ,: ,k1: k2], K[: ,: ,k1: k2])